improvement: faithful, faster demo_circle#16
Merged
Conversation
The `demo_circle` command previously fired waypoint commands on a fixed 150 ms cadence regardless of whether the arm had reached the previous target, so the EE perpetually lagged one waypoint behind the planned trajectory and the traced path bore little resemblance to a circle. The start position `(0.15, 0.0, 0.20)` was also genuinely unreachable within the joint limits — three joints had to clamp to their limit, so the start move never converged. Changes to `command/demo_circle.ex`: - Move the start to `(0.25, 0.0, 0.30)`, which is well inside the joint-limited reachable workspace and lets the entire 30 mm-radius circle stay reachable. - Replace the fixed `Process.sleep/1` between waypoints with `wait_for_arrival/4`, which polls the position estimator's FK until the EE is within `settle_tolerance_m` of the commanded target or `settle_timeout_ms` elapses. - Solve IK directly via `DLS.solve` with the **previous IK solution** as the warm-start seed instead of relying on `Motion.move_to` (whose warm-start gets clobbered by the position estimator during arm motion). Adjacent waypoints differ by ~3° in joint space, so this brings IK convergence from ~17 iterations to 1-3 per waypoint. - Send the resulting joint positions via `BBMotion.send_positions/3` (bypassing IK on the second call). - Tune the IK solver for the warm-start case: `tolerance: 2e-3, step_size: 0.3, lambda: 0.1, max_iterations: 15`. The tight default tolerance (0.1 mm) just wasted iterations when the settle tolerance is 5 mm anyway. - New goal params: `settle_tolerance_m` (default 5 mm) and `settle_timeout_ms` (default 1500 ms). Drops the old `delay` param, which existed only because nothing else governed the cadence. Changes to `robot.ex`: - Bump every joint's `acceleration` limit from `720 °/s²` to `2160 °/s²`. This is consumed only by `BB.Sim.Actuator` when building its trapezoidal motion profile, so it speeds up the visual sim without affecting real-hardware behaviour (the Feetech STS3215 uses its own internal velocity profile). Halves the per-waypoint motion time. End-to-end the sim demo now completes in ~8 s (previously ~25 s with `wait_for_arrival` at the original limits, or producing a non-circular trace at the original 150 ms cadence) and the EE actually visits every commanded point.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
The
demo_circlecommand previously fired waypoint commands on a fixed 150 ms cadence regardless of whether the arm had reached the previous target, so the EE perpetually lagged one waypoint behind the planned trajectory and the traced path bore little resemblance to a circle. The start position(0.15, 0.0, 0.20)was also genuinely unreachable within the joint limits — three joints had to clamp to their limit, so the start move never converged.command/demo_circle.ex(0.25, 0.0, 0.30), well inside the joint-limited reachable workspace.Process.sleep/1between waypoints withwait_for_arrival/4, which polls the position estimator's FK until the EE is withinsettle_tolerance_mof the commanded target orsettle_timeout_mselapses.DLS.solvewith the previous IK solution as the warm-start seed, bypassing the position-estimator pollution ofrobot_statethatMotion.move_tois subject to. Adjacent waypoints differ by ~3° in joint space, so this brings IK convergence from ~17 iterations to 1-3 per waypoint.BB.Motion.send_positions/3.tolerance: 2e-3, step_size: 0.3, lambda: 0.1, max_iterations: 15.settle_tolerance_m(default 5 mm) andsettle_timeout_ms(default 1500 ms). Dropsdelay.robot.exaccelerationlimit from720 °/s²to2160 °/s². Consumed only byBB.Sim.Actuatorfor its trapezoidal motion profile — speeds up the visual sim without affecting real-hardware behaviour (Feetech STS3215 uses its own internal velocity profile).Result
End-to-end the sim demo now completes in ~8 s (previously ~25 s with
wait_for_arrivalat the original acceleration, or producing a non-circular trace at the original 150 ms cadence) and the EE actually visits every commanded point with residuals < 1 mm.Related
The honest reachability reporting that surfaced the unreachable start position depends on beam-bots/bb_ik_dls#24 (DLS now respects joint limits during iteration). Without that fix the buggy start would silently "succeed" with
reached: true. The new start(0.25, 0.0, 0.30)is genuinely reachable on either version, so this PR is not blocked on that one — but the diagnostics path that exposed the bug requires it.Test plan
mix check --no-retrypasses locally (5 tests, 0 failures; credo, dialyzer, formatter, reuse, gettext all clean)